#!/usr/bin/env python
#-*- coding: utf-8 -*-

import socket
import thread
import SocketServer
import json
import re
import string
import sys
import time
import traceback
import  RPi.GPIO as GPIO
import time
#import gpss

# 密文偏移
CAR_ENCODE_BASE_CHAR = '<'
# 允许的字符集
CAR_INFO_LEGAL_REGX_STRING = r"[^\.\da-zA-Z0-9\:\"\,\{\}]+"
CAR_INFO_LEGAL_REGX = re.compile(CAR_INFO_LEGAL_REGX_STRING)
PWMA = 18
AIN1   =  22
AIN2   =  27

PWMB = 23
BIN1   = 25
BIN2  =  24

        
GPIO.setwarnings(False) 
GPIO.setmode(GPIO.BCM)
GPIO.setup(AIN2,GPIO.OUT)
GPIO.setup(AIN1,GPIO.OUT)
GPIO.setup(PWMA,GPIO.OUT)

GPIO.setup(BIN1,GPIO.OUT)
GPIO.setup(BIN2,GPIO.OUT)
GPIO.setup(PWMB,GPIO.OUT)

L_Motor= GPIO.PWM(PWMA,100)
L_Motor.start(0)

R_Motor = GPIO.PWM(PWMB,100)
R_Motor.start(0)


commandQueue = []


def t_up(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,True) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,True) #BIN1
        
        time.sleep(t_time)
        t_stop(0)
                
        
def t_stop(t_time):
        L_Motor.ChangeDutyCycle(0)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(0)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)
        
def t_down(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,True)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,True)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)
        t_stop(0)
        #time.sleep(t_time)
        
def t_left(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,True)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,True) #BIN1
        time.sleep(t_time)
        t_stop(0)
        #time.sleep(t_time)

def t_right(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,True) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,True)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)
        #t_sleep(t_time)   
        t_stop(0)

def replaceChar(index, string, char):
    t = list(string)
    t[index] = char
    return ''.join(t)


def encodeString(jsonString):
    jsonString = CAR_INFO_LEGAL_REGX.sub("", jsonString)
    if len(jsonString) < 21:
        return ""

    infoEncodeString = jsonString
    code = ""
    # 单独加密密文
    for i in range(9, 19):
        currentCh = infoEncodeString[i]
        if currentCh < '0' or currentCh > '9':
            return ""
        code = code + currentCh
        ch = chr(ord(currentCh) - ord('0') + ord(CAR_ENCODE_BASE_CHAR))
        # infoEncodeString[i] = ch
        infoEncodeString = replaceChar(i, infoEncodeString, ch)

    codeIndexArray = [1, 2, 3, 4, 5, 6, 7, 8, 9, 0]
    infoEncodeStringLen = len(infoEncodeString)
    index = 0
    for i in range(0, infoEncodeStringLen):
        if i >= 9 and i <= 18:
            continue
        ch = chr(ord(infoEncodeString[i]) + ord(code[index]) - ord('0'))
        index = codeIndexArray[index]
        # infoEncodeString[i] = ch
        infoEncodeString = replaceChar(i, infoEncodeString, ch)

    return infoEncodeString


def decodeString(encodeString):
    if len(encodeString) < 21:
        return ""

    infoEncodeString = encodeString
    code = ""
    # print("----")
    # print(encodeString)
    # 单独解密密文
    for i in range(9, 19):
        currentCh = infoEncodeString[i]
        ch = chr(ord(currentCh) + ord('0') - ord(CAR_ENCODE_BASE_CHAR))
        code = code + ch
        # infoEncodeString[i] = ch
        infoEncodeString = replaceChar(i, infoEncodeString, ch)

    codeIndexArray = [1, 2, 3, 4, 5, 6, 7, 8, 9, 0]
    infoEncodeStringLen = len(infoEncodeString)
    index = 0
    for i in range(0, infoEncodeStringLen):
        if i >= 9 and i <= 18:
            continue
        ch = chr(ord(infoEncodeString[i]) - ord(code[index]) + ord('0'))
        index = codeIndexArray[index]
        # infoEncodeString[i] = ch
        infoEncodeString = replaceChar(i, infoEncodeString, ch)

    return infoEncodeString



queueLock = 0
def client(ip, port, message):
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.connect((ip, port))
    #sock.setblocking(False)
    #try:
    encode = "{}".format(message)
    decode = decodeString(encode)
    #print(encode)
    #print(decode)

        #print(send: "{}".format(message))
    sock.sendall(message.encode('utf-8'))
    sock.shutdown(1)
    response = sock.recv(2048).decode("utf-8")
    sock.shutdown(0)
    jresp = decodeString(response)
    #print("-------")
    print(jresp)
    jresp = "{}".format(jresp.encode("utf-8"))
    #js = json.dumps(jresp)
    jsonObject = json.loads(jresp)
        #if('command' in jsonObject):
        
    if "commands" in jsonObject:
       # print(1)
        commands = jsonObject.get("commands")
        #print(commands)
        #print("------------------11111111111111111111111")
        lens = len(commands)
        #print("lens: ",lens)
        for i in range(0,lens):
            print("i: ",i)
            eachCommand = commands[i]
            if eachCommand.get("command")=="forcebreak":
                queueLock=1
                del commandQueue[:]
                queueLock=0
                continue
            commandQueue.append(eachCommand)
       # print("===================")
       # print(commandQueue)
        #print("===================")
    #finally:
     #   sock.close()

def doCommand(commandName,t,v):

    v1 =  float(v.encode("utf-8"))
    if commandName=="up":
        t_up(v1,t)
    elif commandName=="down":
        t_down(v1,t)
    elif commandName=="left":
        t_left(v1,t)
    elif commandName=="right":
        t_right(v1,t)
    elif commandName=="stop":
        t_stop(t)
                    

def popCommand():
    while 1:
        #print(1)
        if len(commandQueue)>0 and queueLock==0:
            currentCommand = commandQueue.pop(0)
            commandName = currentCommand.get("command")
            t = currentCommand.get("time")
            v = currentCommand.get("other")
            doCommand(commandName,t,v)
        else:
            time.sleep(1)



msg1 = [{"code": "1232223289", "no": "testCar", "lon": 86.064513, "lat": 44.307681, "v": 0.0, "b": 0}]

def getPos(lng,lat):
    return [{"code": "1232223289", "no": "testCar", "lon": lng, "lat": lat, "v": 0.0, "b": 0}]

def car_link_server(ip,port):

    jmsg1 = json.dumps(msg1)
    a = encodeString(jmsg1)
    
    while 1:
        try:
            time.sleep(3)
            client(ip,port,a)
        except:
            pass
            #traceback.print_exc()


if __name__ == "__main__":
    # Port 0 means to select an arbitrary unused port
    HOST = "192.168.43.49"
    PORT = 9090

    
    thread.start_new_thread( car_link_server, (HOST,PORT) )
    
    popCommand()